// --------------------------------------------------------------------------------------------------------------------
// <copyright file="RGBCameraConnect.cs" company="Microsoft Corporation">
// The MIT License (MIT)
// 
// Copyright (c) 2014, Microsoft Corporation
// 
// Permission is hereby granted, free of charge, to any person obtaining a copy
//  of this software and associated documentation files (the "Software"), to deal
//  in the Software without restriction, including without limitation the rights
//  to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
//  copies of the Software, and to permit persons to whom the Software is
//  furnished to do so, subject to the following conditions:
// 
// The above copyright notice and this permission notice shall be included in
//  all copies or substantial portions of the Software.
// 
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
//  IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
//  FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
//  AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
//  LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
//  OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
//  THE SOFTWARE.
// </copyright>
// --------------------------------------------------------------------------------------------------------------------
namespace Microsoft.Robotics.Vision.Runtime.Cameras
{
    using System;
    using System.Runtime.InteropServices;
    using System.Runtime.Serialization;
    using System.Text;
    using System.Text.RegularExpressions;

    using Microsoft.Robotics.Vision;
    using Microsoft.Robotics.Vision.Cameras;
    using Microsoft.Robotics.Vision.Cameras.Webcam;
    
    /// <summary>
    /// Class which interfaces with the RGBCamera interop.
    /// </summary>
    public class RGBCameraConnect
    {
        /// <summary>
        /// Image width in pixels.
        /// </summary>
        private int width;

        /// <summary>
        /// Image height in pixels.
        /// </summary>
        private int height;

        /// <summary>
        /// Frame rate numerator in frames.
        /// </summary>
        private int frameRateNumerator;

        /// <summary>
        /// Frame rate denominator in seconds.
        /// </summary>
        private int frameRateDenominator;
        
        /// <summary>
        /// Subtype of video stream.
        /// </summary>
        private string subType;

        /// <summary>
        /// Friendly name of the camera.
        /// </summary>
        private string friendlyName;

        /// <summary>
        /// The selected RGB camera.
        /// </summary>
        private RGBCamera rgbCamera;

        /// <summary>
        /// Optional symbolic link for the camera.
        /// This is a unique identifier per camera which depends on the USB port the camera is attached to.
        /// It is used to disambiguate between multiple cameras with identical name attached to a machine.
        /// </summary>
        private string symLinkTemplate;

        /// <summary>
        /// RGB Frame ready delegate, which is called when an RGB frame is ready.
        /// </summary>
        private RgbFrameReady rgbFrameReady;

        /// <summary>Initializes a new instance of the RGBCameraConnect class.</summary>
        /// <param name="width">Width in pixels.</param>
        /// <param name="height">Height in pixels.</param>
        /// <param name="frameRateNumerator">Frame rate numerator in frames.</param>
        /// <param name="frameRateDenominator">Frame rate denominator in seconds.</param>
        /// <param name="subType">Subtype of video frame.</param>
        /// <param name="friendlyName">Friendly name of USB web cam. Will be used as Regex expression.</param>
        /// <param name="rgbFrameReady">Delegate which is called when RGB Image frame is ready.</param>
        /// <param name="symLink">Optional symbolic template to match. Useful to disambiguate between cameras</param>
        public RGBCameraConnect(int width, int height, int frameRateNumerator, int frameRateDenominator, string subType, string friendlyName, RgbFrameReady rgbFrameReady, string symLink = null)
        {
            if (frameRateDenominator == 0)
            {
                throw new ArgumentOutOfRangeException("Frame Rate Denominator cannot be 0");
            }

            this.width = width;
            this.height = height;
            this.frameRateNumerator = frameRateNumerator;
            this.frameRateDenominator = frameRateDenominator;
            this.subType = subType;
            this.friendlyName = friendlyName;
            this.rgbFrameReady = rgbFrameReady;
            this.symLinkTemplate = symLink;
        }

        /// <summary>
        /// RGB Frame Ready delegate, which is called when an RGB Frame becomes available
        /// </summary>
        /// <param name="imageFrame">RGB Image Frame (formatted as Yuy2)</param>
        public delegate void RgbFrameReady(ImageFrameYuy2 imageFrame);

        /// <summary>
        /// Gets the Camera.
        /// </summary>
        public RGBCamera Camera
        {
            get
            {
                return this.rgbCamera;
            }
        }

        /// <summary>
        /// Initialize the camera.
        /// </summary>
        public void InitializeCamera()
        {
            RGBCamera.Initialize();
            bool found = false;
            CaptureFormat desiredFormat = null;

            Regex template = null;
            if (false == string.IsNullOrEmpty(this.friendlyName))
            {
                template = new Regex(this.friendlyName, RegexOptions.IgnoreCase | RegexOptions.CultureInvariant);
            }

            Regex symLinkRegex = null;
            if (false == string.IsNullOrEmpty(this.symLinkTemplate))
            {
                symLinkRegex = new Regex(this.symLinkTemplate, RegexOptions.IgnoreCase | RegexOptions.CultureInvariant);
            } 
            
            foreach (var device in RGBCamera.AllDevices)
            {
                if (!device.Attach())
                {
                    continue;
                }

                if ((null == template || template.IsMatch(device.FriendlyName) || this.friendlyName.Equals(device.FriendlyName)) &&
                     (null == symLinkRegex || symLinkRegex.IsMatch(device.SymbolicLink) || this.symLinkTemplate.Equals(device.SymbolicLink)))
                {
                    foreach (CaptureFormat captureFormat in device.Formats)
                    {
                        if (this.width == captureFormat.nWidth
                            && this.height == captureFormat.nHeight                            
                            && this.subType == captureFormat.subType.Name)
                        {
                            // The frame rate denominator is gauranteed to be not 0 in the constructor.
                            // Verify that the device supports the given frame rate or higher.
                            // We explicitly set the frame rate to the given value. 
                            // The MF Source automatically throttles the webcam source to this value.
                            if (((double)this.frameRateNumerator) / this.frameRateDenominator 
                                <= ((double)captureFormat.nFrameRateNumerator) / captureFormat.nFrameRateDenominator)
                            {
                                this.rgbCamera = device;
                                desiredFormat = captureFormat;
                                desiredFormat.nFrameRateNumerator = this.frameRateNumerator;
                                desiredFormat.nFrameRateDenominator = this.frameRateDenominator;
                                found = true;
                                break;
                            }
                        }
                    }
                }

                if (found)
                {
                    break;
                }
            }

            if (!found)
            {
                StringBuilder message = new StringBuilder();

                message.AppendFormat(
                "\n**************************************\nCamera Specification Format Not Found.\nDesired width:{0}\nheight:{1}\nsubtype:{2}\nfriendly name:{3}\n**************************************",
                this.width,
                this.height,
                this.subType,
                this.friendlyName);

                foreach (var device in RGBCamera.AllDevices)
                {
                    if (!device.Attach())
                    {
                        continue;
                    }

                    message.Append("\n*** DETECTED CAMERA ***\n");
                    message.AppendFormat("device friendly name:\"{0}\"\n", device.FriendlyName);
                    message.AppendFormat("AVAILABLE VIDEO FORMATS:\n");
                    foreach (CaptureFormat captureFormat in device.Formats)
                    {
                        message.AppendFormat("{0}x{1},  {2}/{3}Hz, {4}\n", captureFormat.nHeight, captureFormat.nWidth, captureFormat.nFrameRateNumerator, captureFormat.nFrameRateDenominator, captureFormat.subType.Name);
                    }
                }

                throw new ArgumentException(message.ToString());
            }

            // Set the format. 
            this.rgbCamera.CurrentFormat = desiredFormat;
        }

        /// <summary>
        /// Release the camera and related resources.
        /// </summary>
        public void UninitializeCamera()
        {
            if (this.rgbCamera != null)
            {
                this.rgbCamera.Shutdown();
                this.rgbCamera = null;
            }

            RGBCamera.Uninitialize();
        }

        /// <summary>
        /// Initialize a read sample on the camera.
        /// </summary>
        public void CaptureSample()
        {
            if (this.rgbCamera != null)
            {
                this.rgbCamera.CaptureSample(this.ReadSampleCallback);
            }
        }

        /// <summary>
        /// Set an array of camera properties
        /// </summary>
        /// <param name="cameraProperties">The array of camera properties</param>
        public void SetProperties(RGBCameraProperty[] cameraProperties)
        {
            if (null != cameraProperties)
            {
                foreach (RGBCameraProperty prop in cameraProperties)
                {
                    if (false == this.SetProperty(prop.ControlPropertyName, prop.Value, prop.ControlPropertyFlag))
                    {
                        throw new ArgumentException(
                            string.Format(
                            "Cannot set RGB camera property {0} with value {1} and control flag {2}",
                            prop.ControlPropertyName,
                             prop.Value,
                             prop.ControlPropertyFlag));
                    }
                }
            }
        }

        /// <summary>
        /// Get an array of camera properties
        /// </summary>
        /// <param name="cameraProperties">The array of camera properties to get</param>
        /// <returns>Camera properties</returns>
        public RGBCameraProperty[] GetProperties(RGBCameraProperty[] cameraProperties)
        {
            RGBCameraProperty[] props = null;

            if (null != cameraProperties)
            {
                props = new RGBCameraProperty[cameraProperties.Length];

                for (int i = 0; i < cameraProperties.Length; ++i)
                {
                    int value = 0;
                    int flag = 0;
                    RGBCameraProperty prop = cameraProperties[i];
                    if (false == this.rgbCamera.GetProperty(prop.ControlPropertyName, ref value, ref flag))
                    {
                        throw new ArgumentException(
                            string.Format(
                            "Cannot get RGB camera property {0}",
                            prop.ControlPropertyName));
                    }

                    props[i] = new RGBCameraProperty(prop.ControlPropertyName, value, (ManagedCameraControlPropertyFlags)flag);
                }
            }

            return props;
        }

        /// <summary>
        /// Sets a property on the camera
        /// </summary>
        /// <param name="propertyName">Property name</param>
        /// <param name="value">Property value</param>
        /// <param name="flag">Property control flag</param>
        /// <returns>True if property is successfully set</returns>
        public bool SetProperty(ManagedCameraControlProperty propertyName, int value, ManagedCameraControlPropertyFlags flag)
        {
            return this.rgbCamera.SetProperty(propertyName, value, flag);
        }

        /// <summary>
        /// Sets Video properties with a work around for failure with some webcams that incorrectly set properties
        /// when requested properties match the current values.
        /// Current restriction is that first requested property must not be maximum value
        /// </summary>
        /// <param name="videoProperties">The array of camera properties</param>
        public void SetVideoPropertiesWithInitialization(RGBVideoProperty[] videoProperties)
        {
            if (null != videoProperties && videoProperties.Length > 0)
            {
                RGBVideoProperty[] props = this.GetProperties(videoProperties);
                if (props[0].Value == videoProperties[0].Value)
                {
                    props[0].Value = videoProperties[0].Value + 1;
                    this.SetProperties(props);
                }
                else
                {
                    this.SetProperties(videoProperties);
                }
            }
        }

        /// <summary>
        /// Set an array of video properties
        /// </summary>
        /// <param name="videoProperties">The array of camera properties</param>
        public void SetProperties(RGBVideoProperty[] videoProperties)
        {
            if (null != videoProperties)
            {
                foreach (RGBVideoProperty prop in videoProperties)
                {
                    if (false == this.rgbCamera.SetProperty(prop.VideoPropertyName, prop.Value, prop.VideoPropertyFlag))
                    {
                        throw new ArgumentException(
                            string.Format(
                            "Cannot set RGB video property {0} with value {1} and control flag {2}",
                            prop.VideoPropertyName,
                             prop.Value,
                             prop.VideoPropertyFlag));
                    }
                }
            }
        }

        /// <summary>
        /// Get an array of video properties
        /// </summary>
        /// <param name="videoProperties">The array of camera properties</param>
        /// <returns>Video properties</returns>
        public RGBVideoProperty[] GetProperties(RGBVideoProperty[] videoProperties)
        {
            RGBVideoProperty[] props = null;

            if (null != videoProperties)
            {
                props = new RGBVideoProperty[videoProperties.Length];

                for (int i = 0; i < videoProperties.Length; ++i)
                {
                    int value = 0;
                    int flag = 0;
                    RGBVideoProperty prop = videoProperties[i];
                    if (false == this.rgbCamera.GetProperty(prop.VideoPropertyName, ref value, ref flag))
                    {
                        throw new ArgumentException(
                            string.Format(
                            "Cannot get RGB video property {0}",
                            prop.VideoPropertyName));
                    }

                    props[i] = new RGBVideoProperty(prop.VideoPropertyName, value, (VideoPropertyFlags)flag);
                }
            }

            return props;
        }

        /// <summary>
        /// Call back function for capturing a sample.
        /// </summary>
        /// <param name="data">Data containing image frame as YUY2 format.</param>
        /// <param name="length">Number of bytes in the image frame, which is twice the width * height.</param>
        private void ReadSampleCallback(IntPtr data, int length)
        {
            if (length != this.width * this.height * 2)
            {
                throw new ArgumentException("YUY2 length should be equal to 2 * width * height");
            }

            if (this.rgbFrameReady != null)
            {
                ImageFrameYuy2 imageFrame = ImageOperations2DInterop.CreateImageFrameYuy2FromRawDataBuffer(this.width, this.height, data, length);
                this.rgbFrameReady(imageFrame);
            }
        }
    }
}
